//
// Created by 13562 on 2024/2/3.
//

#include "ImuHandler.h"
#ifdef ENABLE_GLOG
#include "glog/logging.h"
#endif

using namespace std;

void ImuHandler::_handle() {
	ifstream ifs(_file);
	assert(ifs.good());

	vector<DataSeries> ds = _msgInfo.data_series;
	unsigned int seq = _msgInfo.seq_start;
	const unsigned int seq_end = _msgInfo.seq_end;

	assert(seq < seq_end || seq_end == 0);

	istringstream iss;
	string line, item;
	vector<string> items;

//	读取标题行
	getline(ifs, line);
	while (!ifs.eof()) {
		if (seq > seq_end && seq_end != 0) {
			cout << "At the end of sequence." << endl;
			break;
		}
		assert(!_halt);
		if (_halt) {
			cout << "Received g_halt signal." << endl;
			break;
		}

		iss.clear();
		items.clear();
		getline(ifs, line);
		iss.str(line);
		while (getline(iss, item, ',')) {
			items.emplace_back(item);
		}

		sensor_msgs::Imu msg;
		std_msgs::Header header;
		header.frame_id = _msgInfo.frame_id;
		header.seq = seq++;

		for (size_t i = 0; i < ds.size(); i++) {
			if (line.empty()) break;
			switch (ds[i]) {
				case ts:
					header.stamp.sec = stoi(items[i].substr(0, 10));
					header.stamp.nsec = stoi(items[i].substr(10, 19));
					break;
				case acc_x:
					msg.linear_acceleration.x = stod(items[i]);
					break;
				case acc_y:
					msg.linear_acceleration.y = stod(items[i]);
					break;
				case acc_z:
					msg.linear_acceleration.z = stod(items[i]);
					break;
				case ang_x:
					msg.angular_velocity.x = stod(items[i]);
					break;
				case ang_y:
					msg.angular_velocity.y = stod(items[i]);
					break;
				case ang_z:
					msg.angular_velocity.z = stod(items[i]);
					break;
				case ori_x:
					msg.orientation.x = stod(items[i]);
					break;
				case ori_y:
					msg.orientation.y = stod(items[i]);
					break;
				case ori_z:
					msg.orientation.z = stod(items[i]);
					break;
				case ori_w:
					msg.orientation.w = stod(items[i]);
					break;
			}
		}
		msg.header = header;
		boost::array<double, 9> def_cov0 = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
		boost::array<double, 9> def_cov1 = {99999.9, 0.0, 0.0, 0.0, 99999.9, 0.0, 0.0, 0.0, 99999.9};

		msg.linear_acceleration_covariance = def_cov0;
		msg.linear_acceleration_covariance = def_cov0;
		msg.linear_acceleration_covariance = def_cov1;

		_msgQueue.push(msg);
	}
	if (ifs.eof()) {
		cout << "At the end of file." << endl;
	}
	ifs.close();
	cout << "File reading thread exited." << endl;
	_halt = true;
}

void ImuHandler::handle(const std::string& file, const MsgInfo& msgInfo) {
	this->_file = file;
	this->_msgInfo = msgInfo;
	this->_halt = false;
	sensor_msgs::Imu _empty;
	_msgQueue.push(_empty);
	thread handleThread(&ImuHandler::_handle, this);
	handleThread.detach();
}

unsigned long ImuHandler::getQueueLength() {
	return _msgQueue.size();
}

sensor_msgs::Imu ImuHandler::popMsg() {
	if (getQueueLength() == 0) {
		return {};
	}
	sensor_msgs::Imu msg = _msgQueue.front();
	_msgQueue.pop();
	return msg;
}

void ImuHandler::halt() {
	_halt = true;
}

bool ImuHandler::isReadingFile() {
	return !_halt;
}

unsigned long ImuHandler::count() {
	return _count++;
}
